package com.gunncs.actoriface;
import com.gunncs.actoriface.*;
public class RangeFinderAnalysis {
  public static final double DANGERDISTANCE = 1.0;
  public static final double GAPTOLERANCE=10.0;  //TODO figure tolerance for gap and plateau
  public static final double PLATTOLERANCE=10.0;
  public static final double SUBSIDETOLERANCE=10.0;
  
  public static boolean risingOrFalling(boolean rising, double[][] sensorstates) {
    // sensors states is a HISTORYSIZE(3) x2
    double deltaY1 = sensorstates[1][0] - sensorstates[0][0];
    double deltaT1 = sensorstates[1][1] - sensorstates[0][1];
    double deltaY2 = sensorstates[2][0] - sensorstates[1][0];
    double deltaT2 = sensorstates[2][1] - sensorstates[1][1];
    double slope1 = deltaY1 / deltaT1;
    double slope2 = deltaY2 / deltaT2;

    double size= (slope2 - slope1) / (sensorstates[2][1] - sensorstates[0][1]);
    
    boolean result;
    if(rising) result = (size >= GAPTOLERANCE);
    else result = (size <= -GAPTOLERANCE);
    
    return result;
  }

  /**
   * Given a direction to rotate in and an absolute direction,
   * 
   */
  public static int rotateDirection(int direction, boolean clockwise) {
    if(clockwise){
      switch(direction){
        case Decision.NORTH:
          return Decision.EAST;
        case Decision.SOUTH:
          return Decision.WEST;
        case Decision.EAST:
          return Decision.SOUTH;
        case Decision.WEST:
          return Decision.NORTH;
        default:
          throw new RuntimeException("This will never happen! (rotating a non-direction)");
      }
    }else {
      switch(direction){
        case Decision.SOUTH:
          return Decision.EAST;
        case Decision.NORTH:
          return Decision.WEST;
        case Decision.EAST:
          return Decision.NORTH;
        case Decision.WEST:
          return Decision.SOUTH;
        default:
          throw new RuntimeException("This will never happen! (rotating in a non-direction)");
      }
    }
  }

 
}


